#include "stdafx.h"
#include "ImageProcessing.h"

//#define VERBOSE



namespace OTPF {
	namespace Marker {
	arma::mat jacobian (	arma::colvec		&x,		// Positionshypothese
							arma::colvec		&c,		// Kamerakonstante / Brennweite
							std::vector<arma::mat> &R,   // Rotationsmatrix
							std::vector<arma::colvec> &T // Translationsvektor Projektionszentrum
						) {
		#ifdef OLD
		std::vector<arma::colvec> dx (2);
		dx[0] = arma::colvec(2);
		dx[1] = arma::colvec(2);
		dx[0](0) = x(0) - T[0](0);
		dx[0](1) = x(1) - T[0](1);
		dx[0](2) = x(2) - T[0](2);
		dx[1](0) = x(0) - T[1](0);
		dx[1](1) = x(1) - T[1](1);
		dx[1](2) = x(2) - T[1](2);
		
		std::vector<double> N(2);
			for (int k = 0; k < 2; k++) {
			N[k] = R[k](0,2) * dx[k](0) + R[k](1,2) * dx[k](1) + R[k](2,2) * dx[k](2);
		}
			
		arma::mat J = arma::zeros<arma::mat>(4,3);
	J(0,0) = c(0)*R[0](0,0) / N[0] - c(0) * (R[0](0,0)*(dx[0](0))+R[0](1,0)*(dx[0](1))+R[0](2,0)*(dx[0](2)))*R[0](0,2) / (N[0]*N[0]); 
	J(0,1) = c(0)*R[0](1,0) / N[0] - c(0) * (R[0](0,0)*(dx[0](0))+R[0](1,0)*(dx[0](1))+R[0](2,0)*(dx[0](2)))*R[0](1,2) / (N[0]*N[0]);
	J(0,2) = c(0)*R[0](2,0) / N[0] - c(0) * (R[0](0,0)*(dx[0](0))+R[0](1,0)*(dx[0](1))+R[0](2,0)*(dx[0](2)))*R[0](2,2) / (N[0]*N[0]);

	J(1,0) = c(0)*R[0](0,1) / N[0] - c(0) * (R[0](0,1)*(dx[0](0))+R[0](1,1)*(dx[0](1))+R[0](2,1)*(dx[0](2)))*R[0](0,2) / (N[0]*N[0]);
	J(1,1) = c(0)*R[0](1,1) / N[0] - c(0) * (R[0](0,1)*(dx[0](0))+R[0](1,1)*(dx[0](1))+R[0](2,1)*(dx[0](2)))*R[0](1,2) / (N[0]*N[0]);
	J(1,2) = c(0)*R[0](2,1) / N[0] - c(0) * (R[0](0,1)*(dx[0](0))+R[0](1,1)*(dx[0](1))+R[0](2,1)*(dx[0](2)))*R[0](2,2) / (N[0]*N[0]);

	J(2,0) = c(1)*R[1](0,0) / N[1] - c(1) * (R[1](0,0)*(dx[1](0))+R[1](1,0)*(dx[1](1))+R[1](2,0)*(dx[1](2)))*R[1](0,2) / (N[1]*N[1]);
	J(2,1) = c(1)*R[1](1,0) / N[1] - c(1) * (R[1](0,0)*(dx[1](0))+R[1](1,0)*(dx[1](1))+R[1](2,0)*(dx[1](2)))*R[1](1,2) / (N[1]*N[1]);
	J(2,2) = c(1)*R[1](2,0) / N[1] - c(1) * (R[1](0,0)*(dx[1](0))+R[1](1,0)*(dx[1](1))+R[1](2,0)*(dx[1](2)))*R[1](2,2) / (N[1]*N[1]);

	J(3,0) = c(1)*R[1](0,1) / N[1] - c(1) * (R[1](0,1)*(dx[1](0))+R[1](1,1)*(dx[1](1))+R[1](2,1)*(dx[1](2)))*R[1](0,2) / (N[1]*N[1]);
	J(3,1) = c(1)*R[1](1,1) / N[1] - c(1) * (R[1](0,1)*(dx[1](0))+R[1](1,1)*(dx[1](1))+R[1](2,1)*(dx[1](2)))*R[1](1,2) / (N[1]*N[1]);
	J(3,2) = c(1)*R[1](2,1) / N[1] - c(1) * (R[1](0,1)*(dx[1](0))+R[1](1,1)*(dx[1](1))+R[1](2,1)*(dx[1](2)))*R[1](2,2) / (N[1]*N[1]);
			
		mexPrintMatrix(J, "J =");
		
		return J;
		
		#else
			double dx1 = x(0) - T[0](0);
			double dy1 = x(1) - T[0](1);
			double dz1 = x(2) - T[0](2);
			double dx2 = x(0) - T[1](0);
			double dy2 = x(1) - T[1](1);
			double dz2 = x(2) - T[1](2);
			
			double r111 = R[0](0,0);
			double r121 = R[0](0,1);
			double r131 = R[0](0,2);
			double r211 = R[0](1,0);
			double r221 = R[0](1,1);
			double r231 = R[0](1,2); 
			double r311 = R[0](2,0); 
			double r321 = R[0](2,1);
			double r331 = R[0](2,2);
			
			double r112 = R[1](0,0);
			double r122 = R[1](0,1);
			double r132 = R[1](0,2);
			double r212 = R[1](1,0);
			double r222 = R[1](1,1);
			double r232 = R[1](1,2); 
			double r312 = R[1](2,0); 
			double r322 = R[1](2,1);
			double r332 = R[1](2,2);
			
			double kx1 = r111 * dx1 + r211 * dy1 + r311 * dz1;
			double kx2 = r112 * dx2 + r212 * dy2 + r312 * dz2;
			double ky1 = r121 * dx1 + r221 * dy1 + r321 * dz1;
			double ky2 = r122 * dx2 + r222 * dy2 + r322 * dz2;
		    
			double N1 = r131*(dx1)+r231*(dy1)+r331*(dz1);
			double N2 = r132*(dx2)+r232*(dy2)+r332*(dz2);
			
			arma::mat J = arma::zeros<arma::mat> (4,3);
			
			J(0,0) = -c(0)/(N1*N1) * (r131 * kx1 - r111 * N1);
			J(0,1) = -c(0)/(N1*N1) * (r231 * kx1 - r211 * N1);
			J(0,2) = -c(0)/(N1*N1) * (r331 * kx1 - r311 * N1);
			
			J(1,0) = -c(0)/(N1*N1) * (r131 * ky1 - r121 * N1);
			J(1,1) = -c(0)/(N1*N1) * (r231 * ky1 - r221 * N1);
			J(1,2) = -c(0)/(N1*N1) * (r331 * ky1 - r321 * N1);
			
			J(2,0) = -c(1)/(N2*N2) * (r132 * kx2 - r112 * N2);
			J(2,1) = -c(1)/(N2*N2) * (r232 * kx2 - r212 * N2);
			J(2,2) = -c(1)/(N2*N2) * (r332 * kx2 - r312 * N2);
			
			J(3,0) = -c(1)/(N2*N2) * (r132 * ky2 - r122 * N2);
			J(3,1) = -c(1)/(N2*N2) * (r232 * ky2 - r222 * N2);
			J(3,2) = -c(1)/(N2*N2) * (r332 * ky2 - r322 * N2);
			
			return J;
		#endif
			
		}
	}
}